#include "../Device/motor.h"

#include <math.h>

#define M_PI (3.14159265358979323846)
#define RATIO (5 * 19)
#define SPEED ((0.785f * sin(1.884f * TIME) + 1.305f) * RATIO)

extern uint32_t system_time_ms;
extern uint32_t system_time_s;

#ifdef DEBUG
uint8_t buff[8] = {0};
Motor_t motor;
#else
static uint8_t buff[8] = {0};
static Motor_t motor;
#endif

void Motor_Init() {
  BSP_CAN_SetFilter(&hcan, MOTOR_FEEDBACK_ID);
  BSP_CAN_Init(&hcan);
  motor.pid_speed.iMin = -1000;
  motor.pid_speed.iMAX = 1000;
  motor.pid_speed.pid_Freq = 1000;
  motor.pid_speed.pid_K = -1.0f;
  motor.pid_speed.pid_P = 0.000015f;
  motor.pid_speed.pid_I = 0.000001f;
  motor.pid_speed.pid_D = 0.0f;
}

void Motor_Output(float value) {
  if (value > 1) value = 1;
  if (value < -1) value = -1;
  int16_t tmp = value * MOTOR_MAX_CURRENT;
  ((int16_t*)buff)[0] = tmp;
  BSP_CAN_SendData(&hcan, MOTOR_CONTROL_ID, 8, buff);
  Filter_LowPassInit(&motor.filter_speed, 0.25);
}

void Motor_PraseData() {
  if (BSP_CAN_DataReady()) {
    uint8_t* tmp = BSP_CAN_GetData();
    motor.feedback.fb_angle =
        ((tmp[0] << 8) + tmp[1]) / MOTOR_MAX_ANGLE * 360.0f;
    motor.feedback.fb_speed =
        Filter_LowPassApply(&motor.filter_speed,
                            ((int16_t)((tmp[2] << 8) + tmp[3])) / 30.0f * M_PI);
    motor.feedback.fb_current = (tmp[4] << 8) + tmp[5];
    motor.feedback.fb_temperature = tmp[6];
  }
}

void Motor_Control(bool anti) {
  if (anti)
    motor.setpoint = -SPEED;
  else
    motor.setpoint = SPEED;
  if (motor.feedback.fb_speed > SPEED * 2 ||
      motor.feedback.fb_speed < -SPEED * 2) {
    Motor_Output(0.0f);
    NVIC_SystemReset();
  }
  motor.out =
      PID_Update(&motor.pid_speed, motor.feedback.fb_speed, motor.setpoint);
  Motor_Output(motor.out);
}

float Motor_GetSetpoint() { return motor.setpoint; }
float Motor_GetFeedback() { return motor.feedback.fb_speed; }
